"""The evaluation demo of huawei-octopus."""
import json
import os
import struct
from logging import getLogger
from pathlib import Path
from typing import Optional

from huawei_osi.octopus import (
    Evaluation,
    EvaluationMetric,
    EvaluationMetricAnomaly,
    EvaluationMetricAnomalyPointType,
    EvaluationMetricAnomalySubtype,
    EvaluationMetricResult,
    Source,
    Visualization,
    VisualizationStatistic,
    VisualizationStatisticType,
    VisualizationVector,
    VisualizationVectorType,
)
from huawei_osi.sim_osi_pb2 import SimData
from osi3.osi_common_pb2 import Vector3d
from osi3.osi_lane_pb2 import Lane
from osi3.osi_groundtruth_pb2 import GroundTruth

LaneType = Lane.Classification.Type  # type: ignore
REACH_DESTINATION_RADIUS = int(os.getenv('REACH_DESTINATION_RADIUS', '2'))

LOG = getLogger(__name__)


def get_frames(input_: str) -> list[GroundTruth]:
    if input_.endswith('.osi'):
        frames = []
        with open(input_, 'rb') as f:
            while True:
                try:
                    pre_buffer = f.read(4)
                    length = struct.pack('i', pre_buffer)[0]
                    frame_data = f.read(length)
                    frame = GroundTruth.FromString(frame_data)

                except Exception:
                    break
                frames.append(frame)
        return frames
    else:
        with open(input_, 'rb') as f:
            data = SimData.FromString(f.read())
        return data.frames


def evaluate(input_: Path, output: Optional[Path]):
    """Evaluation the algorithm with Octopus OSI data."""
    LOG.info("Read sim.osi.pb to SimData.")
    
    # with open(input_, 'rb') as file:
    #     data = SimData.FromString(file.read())  # type: ignore
    frames = get_frames(input_)

    LOG.info('Start to evaluate %s frames.', len(frames))
    
    # 此处添加评测处理逻辑

    # TODO 确认新版本osi 获取数据的方式
    # destination = data.destination
    is_reached = True
    # is_on_road = True
    is_collide = True
    is_braking = True
    sim_times = []
    acc_x_list = []

    for frame in frames:
        # is_reached = is_reached or evaluate_reach_destination(frame, destination)
        # is_on_road = is_on_road and evaluate_on_road(frame)
        is_collide = evaluate_collision(frame)
        is_braking = is_braking and evaluate_braking(frame)
        sim_times.append(frame.timestamp.seconds + frame.timestamp.nanos * 1e-9)
        acc_x_list.append(get_acc_x(frame))
        
    score = cal_score(is_reached, is_collide, is_braking)
    
    # 输出评测结果
    
    result = Evaluation(
        version=2,
        score=score,
        # TODO 确认新版本osi获取距离跟平均速度
        # distance=data.distance,
        distance=1000,
        # avg_speed=data.distance/(data.frames[-1].timestamp.seconds + 1),
        avg_speed=23,
        metrics=[
            EvaluationMetric(
                status=EvaluationMetricResult.RESULT_PASSED if is_reached else EvaluationMetricResult.RESULT_FAILED,
                anomalies=[
                    EvaluationMetricAnomaly(
                        status=EvaluationMetricResult.RESULT_PASSED if is_reached else EvaluationMetricResult.RESULT_FAILED,
                        subtype=EvaluationMetricAnomalySubtype.SUBTYPE_STOP_SIGN,
                        point_type=EvaluationMetricAnomalyPointType.POINT_TYPE_ALL,
                        points=[4],
                        display_name='到达终点检测'
                    )
                ],
                display_name='到达终点'
            ),
            # EvaluationMetric(
            #     status=EvaluationMetricResult.RESULT_PASSED if is_on_road else EvaluationMetricResult.RESULT_FAILED,
            #     anomalies=[
            #         EvaluationMetricAnomaly(
            #             status=EvaluationMetricResult.RESULT_PASSED if is_on_road else EvaluationMetricResult.RESULT_FAILED,
            #             subtype=EvaluationMetricAnomalySubtype.SUBTYPE_UNSPECIFIED,
            #             point_type=EvaluationMetricAnomalyPointType.POINT_TYPE_ALL,
            #             points=[4],
            #             display_name='在路检测'
            #         )
            #     ],
            #     display_name='在路'
            # ),
            EvaluationMetric(
                status=EvaluationMetricResult.RESULT_PASSED if is_collide else EvaluationMetricResult.RESULT_FAILED,
                anomalies=[
                    EvaluationMetricAnomaly(
                        status=EvaluationMetricResult.RESULT_PASSED if is_collide else EvaluationMetricResult.RESULT_FAILED,
                        subtype=EvaluationMetricAnomalySubtype.SUBTYPE_UNSPECIFIED,
                        point_type=EvaluationMetricAnomalyPointType.POINT_TYPE_ALL,
                        points=[4],
                        display_name='碰撞检测'
                    )
                ],
                display_name='碰撞'
            ),
            EvaluationMetric(
                status=EvaluationMetricResult.RESULT_PASSED if is_braking else EvaluationMetricResult.RESULT_FAILED,
                anomalies=[
                    EvaluationMetricAnomaly(
                        status=EvaluationMetricResult.RESULT_PASSED if is_braking else EvaluationMetricResult.RESULT_FAILED,
                        subtype=EvaluationMetricAnomalySubtype.SUBTYPE_UNSPECIFIED,
                        point_type=EvaluationMetricAnomalyPointType.POINT_TYPE_ALL,
                        points=[4],
                        display_name='急刹检测'
                    )
                ],
                display_name='急刹'
            ),
        ],
        vis=Visualization(
            sim_times=sim_times,
            vectors=[
                VisualizationVector(
                    values=acc_x_list,
                    display_name='accX'
                )
            ]
        ),
        source=Source.SOURCE_CUSTOMIZED_OFFLINE
    )
    
    LOG.info(
        'End to evaluate. score: %s, distance: %s, average speed: %s',
        result.score, result.distance, result.avg_speed
    )
    if output:
        try:
            res=bytes(result)
        except Exception:
            LOG.error('Serialize evaluation result to eva.pb fail: %s', result)
            raise
        with open(output, 'wb') as out:
            out.write(res)
    else:
        dict_ = result.to_dict(include_default_values=True)
        del dict_['vis']
        LOG.info(json.dumps(dict_, indent=2))
        

def cal_score(is_reached, is_collide, is_braking):
    """A simple example of calculating score."""
    reach_score = 0 if is_reached else 10
    # road_score = 0 if is_on_road else 40
    collision_score = 0 if is_collide else 40
    braking_score = 0 if is_braking else 10
    return 100 - reach_score - collision_score - braking_score


def evaluate_reach_destination(
    frame,
    destination: Vector3d,
    reach_radius: int = REACH_DESTINATION_RADIUS,
):   
    """Evaluate whether ego reach destination or not."""
    ego_id = frame.host_vehicle_id
    for moving_object in frame.moving_object:
        if moving_object.id == ego_id:
            distance = _cal_vector_dist(
                moving_object.base.position, destination
            )
            return distance < reach_radius
    return False


# def evaluate_on_road(frame):
#     """Evaluate whether ego on road or not."""
#     assigned_lane_ids = _get_assigned_lane_ids(frame)
#     for lane in frame.lane:
#         if lane.id in assigned_lane_ids:
#             type_ = lane.classification.type
#             if type_ == LaneType.TYPE_NONDRIVING:
#                 return False
#     return True


def evaluate_collision(frame):
    """Evaluate whether ego collide with object or not.

    Args:
        frame: a frame of osi data.
    
    Returns:
        The res of whether ego collided of not.
        
    References:
        https://www.geometrictools.com/Documentation/DynamicCollisionDetection.pdf
        https://stackoverflow.com/questions/47866571/simple-oriented-bounding-box-obb-collision-detection-explaining
    """
    return True
    
    
def evaluate_braking(frame):
    """Evaluate whether ego had emergency braking."""
    ego_id = frame.host_vehicle_id
    for moving_object in frame.moving_object:
        if moving_object.id == ego_id:
            if moving_object.base.acceleration.x < -3:
                return False
    return True


def get_acc_x(frame):
    ego_id = frame.host_vehicle_id
    for moving_object in frame.moving_object:
        if moving_object.id == ego_id:
            return moving_object.base.acceleration.x


# def _get_assigned_lane_ids(frame):
#     """Get assiged lane ids from moving object."""
#     ego_id = frame.host_vehicle_id
#     for moving_object in frame.moving_object:
#         if moving_object.id == ego_id:
#             return moving_object.assigned_lane_id
#     return []


def _cal_vector_dist(src: Vector3d, dest: Vector3d) -> float:
    """Calculate the Euclidean distance of 3D space."""
    return ((src.x - dest.x)**2 + (src.y - dest.y)**2 +
            (src.z - dest.z)**2)**0.5
    
    
if __name__ == '__main__':
    osi_path_env = os.getenv('SIM_OSI_PATH')
    eva_path_env = os.getenv('EVA_PATH')
    if not osi_path_env:
        raise RuntimeError("Environment variable SIM_OSI_PATH is requried.")
    osi_path = Path(osi_path_env)
    eva_path = Path(eva_path_env) if eva_path_env else None
    evaluate(osi_path, eva_path)
